// this is the header file that tells the compiler what pins and ports, etc.
 // are available on this chip.
 #include <avr/io.h>
 #include <avr/interrupt.h>
 #include <stdio.h>
 #include <stdint.h>
 #include "motorControl.h"
 #include "encoder.h"
 #include "sensors.h"
 
 // define what pins the LEDs are connected to.

 //#define Warn PD6
 //#define Red_L PD7
 #define Red PB2
 #define Yellow PB1
 #define Green PB0


 #define FOSC 20000000
 #define BAUD 9600
 #define MYUBRR (((FOSC/16)/BAUD)-1)

// Some macros that make the code more readable
 //#define output_low(port,pin) port &= (uint8_t)~(1<<pin)
 //#define output_high(port,pin) port |= (uint8_t)(1<<pin)
 //#define set_input(portdir,pin) portdir &= (uint8_t)~(1<<pin)
 //#define set_output(portdir,pin) portdir |=(uint8_t)(1<<pin)

 #define set(p,r) ((p) |= (bit(r)))
 #define clear(p,r) ((p) &= ~(bit(r)))
 #define bit(x) (0x01 << (x))
 
 #define startTimer1 set(TCCR1B,CS12)//256 prescale
 #define stopTimer1 clear(TCCR1B,CS12)

 #define startTimer0 TCCR0B|=0x05 //1024 prescale
 #define stopTimer0 TCCR0B&=~0x07
 
 void ioinit(void);      // initializes IO
 static int uart_putchar(char c, FILE *stream);
 uint8_t uart_getchar(void);
 static FILE mystdout = FDEV_SETUP_STREAM(uart_putchar, NULL, _FDEV_SETUP_WRITE);
 void Move(void);

 volatile uint8_t turns = 0;
 volatile uint8_t revs = 0;
 volatile uint8_t TimerCount;
 char DataPacket[4];

 // this is just a program that 'kills time' in a calibrated method
 void delay_ms(uint8_t ms) {
   uint16_t delay_count = F_CPU / 17500;
   volatile uint16_t i;
 
   while (ms != 0) {
     for (i=0; i != delay_count; i++);
     ms--;
   }
 }

 int main(void) {
   // initialize the direction of PORTD #6 to be an output
	uint8_t count;

    ioinit(); //Setup IO pins and defaults  
	init_Motor();
	//int_Encoders();

	sei();
	delay_ms(20);
	output_high(PORTB,Green);
	
	

	set(TCCR0A , WGM01); // enable CTC on timer 0
	set(TIMSK0 , OCIE0A);//enable timer intrupt
	OCR0A=0x98;
		
  while(1){
  		output_high(PORTB,Yellow);

	  	count = 0;
		do{
			if(UCSR0A & (1<<RXC0)){
				DataPacket[count] = uart_getchar();
				printf("Char %c",DataPacket[count]);
				if(DataPacket[0] == 'V'){count++;}
				else{count=0;}
			}//if
		}while(count <4);
		
		Move();

		TimerCount=((DataPacket[1]-64)*8);
		output_high(PORTB,Red);
		startTimer0;
		int temp =0;
		while(TimerCount){temp++;} //wait for timer to expire
		output_low(PORTB,Red);
		stop(10);

		 
	}//while(1)	

 }//main


void ioinit (void){
    set_output(DDRB, Yellow );
   	set_output(DDRB, Red);
   	set_output(DDRB, Green);

    //USART Baud rate: 9600
    UBRR0H = (MYUBRR>>8);
    UBRR0L = MYUBRR;
    UCSR0B = (1<<RXEN0)|(1<<TXEN0);
    
    stdout = &mystdout; //Required for printf init
}

static int uart_putchar(char c, FILE *stream){

    if (c == '\n'){
	loop_until_bit_is_set(UCSR0A, UDRE0);
    UDR0 = '\r';
	} 
 
    loop_until_bit_is_set(UCSR0A, UDRE0);
    UDR0 = c;
    
    return 0;
}

uint8_t uart_getchar(void){
    while( !(UCSR0A & (1<<RXC0)) );
    return(UDR0);
}

void Move(void){
if(DataPacket[0]=='V'){
	if (DataPacket[2] == 'F' && DataPacket[3] == 'F'){
	//forward
		forward(10);
		printf("Forward");
	}
	if (DataPacket[2] == 'R' && DataPacket[3] == 'R'){
	//revrse
		reverse(10);
		printf("Reverse");
	}		
	if (DataPacket[2] == 'R' && DataPacket[3] == 'F'){
	//Left Turn
		leftTurn(10);
		printf("LeftTrn");
	}
	if (DataPacket[2] == 'F' && DataPacket[3] == 'R'){
	//Right Turn
	rightTurn(10);
	printf("RightTn");
	}
}
}


ISR(TIMER0_COMPA_vect){
TimerCount--;
if(TimerCount==0){stopTimer0;}
}


ISR(PCINT2_vect){
	turns++;
	if(turns >=140){
		highBrake(10);
		//printf("Stop!, Turns = %d, Revs = %d\n", turns, revs);
		turns = 0;
		revs = 0;
	}

	}
